#include "kinect_gui.h"
#include "ui_kinect_gui.h"
#include <pcl/visualization/cloud_viewer.h>

// PCL Visualization stuff
pcl::visualization::PCLVisualizer viewer("Point cloud" ,0);
pcl::visualization::PCLVisualizer viewer_user("usercloud" ,0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_user (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB point_user;
pcl::visualization::PointCloudColorHandlerRGBField< pcl::PointXYZRGB > color_cloud(cloud);
pcl::visualization::PointCloudColorHandlerRGBField< pcl::PointXYZRGB > color_cloud_user(cloud_user);

// Function to check status
#define CHECK_RC(rc, what, flag)\
    flag= 1;\
    if (rc != XN_STATUS_OK)\
{\
    qDebug() << what << "failed :"<< xnGetStatusString(rc);\
    flag = 0;\
    }\
    qDebug() << what << " = " << flag;

KINECT_GUI::KINECT_GUI(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::KINECT_GUI)
{
    ui->setupUi(this);
    nRetVal = XN_STATUS_OK;
    /*rgb_image.create(480,640, CV_8UC3);*/

    n_pixels = XN_VGA_X_RES*XN_VGA_Y_RES;
    cloud->resize(n_pixels);
    proj = new XnPoint3D[ n_pixels ];
    wrld = new XnPoint3D[ n_pixels ];
    filename.clear();
}

KINECT_GUI::~KINECT_GUI()
{
    delete ui;
}

void KINECT_GUI::on_action_Open_triggered()
{
    filename = QFileDialog::getOpenFileName(this, tr("Open File"),
                                            QCoreApplication::applicationDirPath(),
                                            tr("Kinect  Recording (*.oni)"));
    qDebug()<< filename;
}

void KINECT_GUI::on_bt_startVideo_clicked()
{
    if(filename.isEmpty())
    {
        do
        {
            on_action_Open_triggered();
        }while(filename.isEmpty());
    }
    context.Release();
    nRetVal = context.Init();
    CHECK_RC(nRetVal, "Intialization",context_init);

    nRetVal = context.OpenFileRecording( filename.toAscii(), Player);
    CHECK_RC(nRetVal, "Recording opening",context_init);

    //Checking the Recorded File
    nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
    CHECK_RC(nRetVal, "Checking for depth data in recording",depth_init);

    //Checking the Recorded File
    nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, color);
    CHECK_RC(nRetVal, "Checking for RGB data in recording",color_init);

    //Checking the User Segmenetation
    nRetVal = user.Create(context);
    CHECK_RC(nRetVal, "Creating user Detection for recording",user_init);
    if (user.GetSkeletonCap().NeedPoseForCalibration())
    {
        pose_needed =1;
        qDebug() << "Need Pose for Calibration";
    }
    else
    {
        pose_needed =0;
        qDebug() << "Recorder Can detect skeleton WITHOUT pose calibration";
        // Setting Skeleton detection with all the joints
        user.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

        bool callback;
        nRetVal = user.RegisterUserCallbacks(User_NewUser, User_LostUser, this, hUserCallbacks);
        CHECK_RC(nRetVal, "Register to user callbacks", callback);

        nRetVal = user.GetSkeletonCap().RegisterToCalibrationStart(User_Calibration_Start,this,hCalibrationStart);
        CHECK_RC(nRetVal, "Register to calibration_start callback", callback);

        nRetVal = user.GetSkeletonCap().RegisterToCalibrationComplete(User_Calibration_Complete,this,hCalibrationComplete);
        CHECK_RC(nRetVal, "Register to calibration_complete callback", callback);


    }

    bool ready;
    nRetVal = context.StartGeneratingAll();
    CHECK_RC(nRetVal, "StartGenerating",ready);

    if(ready)
    {
        int ret = QMessageBox::information(this, tr("Video PLayer"),
                                           tr("The Video Will Start when you confirm it"
                                              "Please Read the debugging information before advancing\n\n"),
                                           QMessageBox::Ok);

        if( ret == QMessageBox::Ok)
        {

            QMetaObject::invokeMethod(this, "process_frame",
                                      Qt::QueuedConnection);
            //            QTimer::singleShot(1000,this,SLOT(process_frame()));
        }
    }
}



void KINECT_GUI::process_frame()
{

    nRetVal = context.WaitAndUpdateAll();
    if (nRetVal != XN_STATUS_OK)
    {
        qDebug()<< "UpdateData Failed:"<<  xnGetStatusString(nRetVal);
        return;
    }

    //depth
    depth.GetMetaData(depthMD);
    pDepthMap = depthMD.Data();

    //color
    color.GetMetaData(imageMD);
    pImageMap = color.GetRGB24ImageMap();

    //user
    user.GetUserPixels(0,sceneMD);
    pLabel= sceneMD.Data();

    //    convert_to_opencv(&imageMD,pImageMap,&rgb_image);

    // Handling cloudpoints
    convert_to_world(proj,wrld,pDepthMap); // Converting from projection to world cordinates

    // Population CLOUD and User Cloud (user id =1)
    convert_to_PCL(wrld, pImageMap, pLabel, 1);

    // Showing CLOUD in Window
    viewer.removePointCloud("Point cloud"); // it's safe to remove a non-existent cloud
    viewer.addPointCloud(cloud, color_cloud, "Point cloud"); // acloud is a PointCloud::Ptr
    viewer.setBackgroundColor(0, 0, 0.1);
    vtkSmartPointer<vtkRenderWindow> renderWindow = viewer.getRenderWindow();
    ui->qvtkWidget->SetRenderWindow(renderWindow);
    ui->qvtkWidget->update();

    if(user_1_skeleton_available)
    {

        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_HEAD           ,HEAD           );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_NECK           ,NECK           );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_TORSO          ,TORSO          );
        //        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_WAIST          ,WAIST          );

        //        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_COLLAR    ,LEFT_COLLAR    );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_SHOULDER  ,LEFT_SHOULDER  );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_ELBOW     ,LEFT_ELBOW     );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_WRIST     ,LEFT_WRIST     );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_HAND      ,LEFT_HAND      );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_FINGERTIP ,LEFT_FINGERTIP );

        //        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_COLLAR   ,RIGHT_COLLAR   );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_SHOULDER ,RIGHT_SHOULDER );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_ELBOW    ,RIGHT_ELBOW    );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_WRIST    ,RIGHT_WRIST    );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_HAND     ,RIGHT_HAND     );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_FINGERTIP,RIGHT_FINGERTIP);
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_HIP       ,LEFT_HIP       );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_KNEE      ,LEFT_KNEE      );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_ANKLE     ,LEFT_ANKLE     );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_LEFT_FOOT      ,LEFT_FOOT      );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_HIP      ,RIGHT_HIP      );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_KNEE     ,RIGHT_KNEE     );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_ANKLE    ,RIGHT_ANKLE    );
        user.GetSkeletonCap().GetSkeletonJointPosition(1,XN_SKEL_RIGHT_FOOT     ,RIGHT_FOOT     );

        viewer_user.removeAllShapes();


        draw_limbs(& HEAD, & NECK                       , QString("HN"));
        draw_limbs(& NECK, & TORSO                      , QString("NT"));

        draw_limbs(& LEFT_SHOULDER, & LEFT_ELBOW        , QString("LSLE"));
        draw_limbs(& LEFT_ELBOW, & LEFT_HAND            , QString("LELH"));

        draw_limbs(& RIGHT_SHOULDER, &  RIGHT_ELBOW     , QString("RSRE"));
        draw_limbs(& RIGHT_ELBOW, &     RIGHT_HAND     , QString("RERH"));

        draw_limbs(& RIGHT_SHOULDER, & LEFT_SHOULDER       , QString("RSLS"));

        draw_limbs(& TORSO, & RIGHT_HIP                      , QString("TRH"));
        draw_limbs(& RIGHT_HIP, & RIGHT_KNEE                     , QString("RHRK"));
        draw_limbs(& RIGHT_KNEE, & RIGHT_FOOT                    , QString("RKRF"));



        draw_limbs(& TORSO, & LEFT_HIP                      , QString("TLH"));
        draw_limbs(& LEFT_HIP, & LEFT_KNEE                     , QString("LHLK"));
        draw_limbs(& LEFT_KNEE, & LEFT_FOOT                    , QString("LKLF"));



//        viewer_user.resetCamera();




    }
    // Showing UserCLOUD in Window
    viewer_user.removePointCloud("usercloud"); // it's safe to remove a non-existent cloud
    viewer_user.addPointCloud(cloud_user, color_cloud_user, "usercloud"); // acloud is a PointCloud::Ptr
    if(user_1_skeleton_available)      pcl::io::savePCDFile("user.pcd", *cloud_user);
    viewer_user.setBackgroundColor(0.3, 0.3, 0.3);
    vtkSmartPointer<vtkRenderWindow> renderWindowUser = viewer_user.getRenderWindow();
    ui->qvtuser->SetRenderWindow(renderWindowUser);
    ui->qvtuser->update();
    //    QMetaObject::invokeMethod(this, "process_frame",
    //                              Qt::QueuedConnection);
    QTimer::singleShot(20,this,SLOT(process_frame()));

}
void KINECT_GUI::draw_limbs(XnSkeletonJointPosition *j1, XnSkeletonJointPosition *j2, QString name)
{
    viewer_user.addLine (pcl::PointXYZ(j1->position.X/1,j1->position.Y/1,j1->position.Z/1),\
                         pcl::PointXYZ(j2->position.X/1,j2->position.Y/1,j2->position.Z/1),\
                         255,0,0,name.toStdString());
}


void KINECT_GUI::convert_to_world( XnPoint3D* projection , XnPoint3D* world , const  XnDepthPixel*  depthpxl)
{
    int i;

    for (int y=0; y<XN_VGA_Y_RES; y++)
    {
        for(int x=0;x<XN_VGA_X_RES;x++)
        {
            i= y* XN_VGA_X_RES + x;
            projection[i].X = (XnFloat)x;
            projection[i].Y = (XnFloat)y;
            projection[i].Z = depthpxl[i];
        }

    }
    nRetVal = depth.ConvertProjectiveToRealWorld(n_pixels,proj,wrld);
    if (nRetVal != XN_STATUS_OK)
    {
        qDebug()<< "Error in COnversion";
    }

}

void KINECT_GUI::convert_to_PCL (XnPoint3D* world ,  const XnRGB24Pixel* pimgmap , const XnLabel* pLbl , unsigned int usert_id)
{
    int i=0;
    cloud_user->clear();

    user_cloud_populated =0;
    for (int y=0; y<XN_VGA_Y_RES; y++)
    {
        for(int x=0;x<XN_VGA_X_RES;x++)
        {
            i= y* XN_VGA_X_RES + x;

            cloud->points[i].x=world[i].X;
            cloud->points[i].y=world[i].Y;
            cloud->points[i].z=world[i].Z;
            // Coordinates
            cloud->points[i].r = pimgmap[i].nRed;
            cloud->points[i].g = pimgmap[i].nGreen;
            cloud->points[i].b = pimgmap[i].nBlue;
            if(pLbl[i] == usert_id) //user 1 is here
            {
                user_cloud_populated =1;
                cloud_user->push_back(cloud->points[i]);
            }
        }
    }


}
/*
void KINECT_GUI::convert_to_opencv(xn::ImageMetaData *imgMD, const XnRGB24Pixel *pimgmap, cv::Mat *opcv)
{
    for(unsigned int i = 0; i < imgMD->XRes()*imgMD->YRes();i++)
    {
        opcv->data[3*i+0]=pimgmap[i].nBlue;
        opcv->data[3*i+1]=pimgmap[i].nGreen;
        opcv->data[3*i+2]=pimgmap[i].nRed;

    }
    cv::imshow("RGB",*opcv);
}
*/
void KINECT_GUI::callback_called(QString msg)
{
    qDebug()<< "********************* CALLBACK !";
    qDebug()<< "Callback" << msg;
    qDebug()<< "********";


}

void XN_CALLBACK_TYPE KINECT_GUI::User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
    KINECT_GUI *pthis = (KINECT_GUI*)pCookie;
    QString temp;
    temp = QString("User Detected") + QString("%1").arg(nId);

    pthis->callback_called(temp);
    pthis->user.GetSkeletonCap().RequestCalibration(nId, 1);

}


void XN_CALLBACK_TYPE KINECT_GUI::User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
    KINECT_GUI *pthis = (KINECT_GUI*)pCookie;
    pthis->callback_called("User Lost");
    if(nId ==1)
    {
        pthis->user_1_skeleton_available=0;
    }
}


void XN_CALLBACK_TYPE KINECT_GUI::User_Calibration_Start(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie)
{
    KINECT_GUI *pthis = (KINECT_GUI*)pCookie;
    pthis->callback_called("Calibration Started. You should be thinking of celebrating by now");
}

void XN_CALLBACK_TYPE KINECT_GUI::User_Calibration_Complete(xn::SkeletonCapability& capability, XnUserID nId, XnCalibrationStatus calibrationError, void* pCookie)
{
    KINECT_GUI *pthis = (KINECT_GUI*)pCookie;
    if (calibrationError == XN_CALIBRATION_STATUS_OK)
    {
        pthis->user.GetSkeletonCap().StartTracking(nId);
        pthis->callback_called("******* JUMP AROUND - USER CALIBRATED");
        if(nId ==1) // so far we are only tracking one user
        {
            pthis->user_1_skeleton_available =1;

        }
        //       emit pthis->calibrationNotification(nId,CalibrationEndSuccess);
        return;
    }
    // Calibration failed

    pthis->callback_called("******** User Not Calibrated ");

}

